clc;clear;close all;
global uLINK ID

RobotModelInitialization;
ForwardKinematics(ID.BODY);

uLINK(ID.L_LEG_J2).q = angle2radian(-45);
uLINK(ID.R_LEG_J2).q = angle2radian(-45);
uLINK(ID.L_LEG_J3).q = angle2radian(45);
uLINK(ID.R_LEG_J3).q = angle2radian(45);

data_len = 100;
x1=1,     z1=0;    v0=0;    v1=0;    x2=0.8,   z2=0.4;
[t1,qt1,v1]=planeTrajectoryThirdOrder(x1,x2,z1,z2,v0,v1,data_len/4);hold on;
x3=0.2,   z3=0.8;  v0=v1;  v1=0
[t2,qt2,v1]=planeTrajectoryThirdOrder(x2,x3,z2,z3,v0,v1,data_len/4);hold on;
x4=-0.2,  z4=0.4;  v0=v1;  v1=0;
[t3,qt3,v1]=planeTrajectoryThirdOrder(x3,x4,z3,z4,v0,v1,data_len/4);hold on;
x5=-0.8,  z5=0.2;  v0=v1;  v1=0;
[t4,qt4,v1]=planeTrajectoryThirdOrder(x4,x5,z4,z5,v0,v1,data_len/4);hold on;
x=cat(2,t1,t2,t3,t4);
y=linspace(-0.2,-0.2,data_len);
z=cat(2,qt1,qt2,qt3,qt4);
%figure,plot(x,z);grid on;
figure,data_calculate = 0;
for n = 1:data_len
    terminusR.p = [x(1,n) y(1,n) z(1,n)]';
    terminusR.R = rpy2rot(0,angle2radian(0),0);
    InverseKinematics(7,terminusR);
    arr4(1,n) = uLINK(ID.R_LEG_J2).q;
    arr5(1,n) = uLINK(ID.R_LEG_J3).q;
    root = uLINK(ID.R_LEG_J5).p;
    roottemp = root; 
    rootarr2 = roottemp; 
    if (data_calculate == 0)
        rootarr1 = roottemp; 
        data_calculate = data_calculate + 1;
        p1 = plotLink(ID.R_LEG_J2,ID.R_LEG_J3);
        p2 = plotLink(ID.R_LEG_J3,ID.R_LEG_J4);
        plotroot(rootarr1,rootarr2);delete(p1);delete(p2);
        continue;
    else
        p1 = plotLink(ID.R_LEG_J2,ID.R_LEG_J3);
        p2 = plotLink(ID.R_LEG_J3,ID.R_LEG_J4);
        plotroot(rootarr1,rootarr2);
    end
    rootarr1 = roottemp; 
    data_calculate = data_calculate + 1;
    drawnow;delete(p1);delete(p2);
end
figure,a=1,b=100;
plot(linspace(a,b,100),arr4(1,:),'color','r'),hold on;
plot(linspace(a,b,100),arr5(1,:),'color','g'),legend('R_LEG_J2','R_LEG_J3'),title('?????2');grid on;
